/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */

#include "CommunicableMotor.h"
#include "VelocityControlledMotor.h"
#include "TorqueControlledMotor.h"
#include "CorbaImplementations.h"

using std::pair;

template<>
void CommunicableMotor<VelocityControlledMotor>::copyMotor(VMotorOut *vMotor) {
	VelocityControlledMotor *motor = dynamic_cast<VelocityControlledMotor*>(this);
	VVelocityControlledMotorOut *vVelocityControlledMotor = VVelocityControlledMotorOut::_downcast(vMotor);
	vVelocityControlledMotor->desiredVelocity(motor->getDesiredVelocity());
	vVelocityControlledMotor->maxForce(motor->getMaxForce());
	vVelocityControlledMotor->loStop(motor->getLoStop());
	vVelocityControlledMotor->hiStop(motor->getHiStop());
	vVelocityControlledMotor->angle(motor->getAngle());
}

template<>
void CommunicableMotor<TorqueControlledMotor>::copyMotor(VMotorOut *vMotor) {
	TorqueControlledMotor *motor = dynamic_cast<TorqueControlledMotor*>(this);
	VTorqueControlledMotorOut *vTorqueControlledMotor = VTorqueControlledMotorOut::_downcast(vMotor);
	vTorqueControlledMotor->torque(motor->getTorque());
	vTorqueControlledMotor->loStop(motor->getLoStop());
	vTorqueControlledMotor->hiStop(motor->getHiStop());
	vTorqueControlledMotor->angle(motor->getAngle());
}

template<>
void CommunicableMotor<VelocityControlledMotor>::copyMotor(VMotorIn *vMotor) {
	VelocityControlledMotor *motor = dynamic_cast<VelocityControlledMotor*>(this);
	VVelocityControlledMotorIn *vVelocityControlledMotor = VVelocityControlledMotorIn::_downcast(vMotor);
	motor->setDesiredVelocity(vVelocityControlledMotor->desiredVelocity());
	motor->setMaxForce(vVelocityControlledMotor->maxForce());
}

template<>
void CommunicableMotor<TorqueControlledMotor>::copyMotor(VMotorIn *vMotor) {
	TorqueControlledMotor *motor = dynamic_cast<TorqueControlledMotor*>(this);
	VTorqueControlledMotorIn *vTorqueControlledMotor = VTorqueControlledMotorIn::_downcast(vMotor);
	motor->setTorque(vTorqueControlledMotor->torque());
}

template<>
pair<VMotorOut*, VMotorIn*> CommunicableMotor<VelocityControlledMotor>::activateMotor() {
	VelocityControlledMotor *motor = dynamic_cast<VelocityControlledMotor*>(this);
	VVelocityControlledMotorOut *vVelocityControlledMotorOut = new VVelocityControlledMotorOut_i(motor->getAngle(), motor->getLoStop(), motor->getHiStop(), motor->getDesiredVelocity(), motor->getMaxForce());
	VVelocityControlledMotorIn *vVelocityControlledMotorIn = new VVelocityControlledMotorIn_i(motor->getDesiredVelocity(), motor->getMaxForce());
	return std::make_pair(vVelocityControlledMotorOut, vVelocityControlledMotorIn);
}

template<>
pair<VMotorOut*, VMotorIn*> CommunicableMotor<TorqueControlledMotor>::activateMotor() {
	TorqueControlledMotor *motor = dynamic_cast<TorqueControlledMotor*>(this);
	VTorqueControlledMotorOut *vTorqueControlledMotorOut = new VTorqueControlledMotorOut_i(motor->getAngle(), motor->getLoStop(), motor->getHiStop(), motor->getTorque());
	VTorqueControlledMotorIn *vTorqueControlledMotorIn = new VTorqueControlledMotorIn_i(motor->getTorque());
	return std::make_pair(vTorqueControlledMotorOut, vTorqueControlledMotorIn);
}
